#include "cequationutils.h"

/**
 * solve - solves system of linear equations with Gauss-Jordan elimination
 * @param m left side of system
 * @param v right side of system
 * @param x particular result
 * @param so solution space
 * @return number of solutions
 */
template <typename T> int CEquationUtils<T>::solve(const CMatrix<T> &m,
    const CVector<T> &v, CVector<T> &x, CMatrix<T> &so) {

  // Clear result matrix
  so.clearToZero();

  // Create matrix of system (m|v), convert it to upper triangular form and
  // get number of it's linear dependent (LD) columns
  CMatrix<T> extM = CMatrix<T>(m, v, CMatrix<T>::VECTOR_COL);
  TIndex depCount = extM.doGaussElimination();

  // Split upper triangular matrix to left (tmpM) and right (tmpV) side
  CMatrix<T> tmpM(extM.subMatrix(0, 0, m.getNumOfRows(), m.getNumOfCols()));
  CVector<T> tmpV(extM.getCol(m.getNumOfCols()));

  // If matrix of system (m|v) has same number of LD as matrix of left side
  // => system hasn't solution (rank of both matrices must be same => right
  // side must be LD)
  if (depCount == tmpM.doGaussElimination()) {
    return EQUATION_SOLUTION_NONE;
  }

  // Here..matrix of left side has (depCount - 1) LD columns
  // If (depCount - 1) is zero than system has exactly one solution => finish
  // elimination of matrix of system
  if (!(--depCount)) {
    extM.toLowerTriangular();
    x = extM.getCol(m.getNumOfCols());
    return EQUATION_SOLUTION_ONE;
  }

  // Here ..system has infinite solutions, find particular solution and
  // solution space
  x = CVector<T>(m.getNumOfCols());
  so = CMatrix<T>(m.getNumOfCols(), depCount);
  findAllSolutions(tmpM, tmpV, x, so);

  return EQUATION_SOLUTION_INF;
}
//---------------------------------------------------------------------------

/**
 * solve - solves system of linear equations with LU decomposition
 * @param m left side of system
 * @param v right side of system
 * @param x result of system
 */
template <typename T> int CEquationUtils<T>::solveLU(const CMatrix<T> &m,
    const CVector<T> &v, CVector<T> &x) {

  // Decomposition of matrix of left side
  CMatrix<T> l, u;
  m.doLUDecomposition(l, u);

  // If u has zero on diagonal, system hasn't solution (l has diagonal made
  // from ones)
  for (TIndex i = 0; i < u.getNumOfRows(); i++) {
    if (CMath<T>::equals(0, u.getItemsPtr()[i][i])) {
      return EQUATION_SOLUTION_NONE;
    }
  }

  // Solve two systems of linear equations with triangular matrices
  // ly = v, ux = y ... mx = v => (lu)x = v => l(ux) = v => ly = v, ux = y
  CVector<T> y;
  findSolutionL(l, v, y);
  findSolutionU(u, y, x);
  return EQUATION_SOLUTION_ONE;
}
//---------------------------------------------------------------------------

/**
 * findAllSolutions - finds all solution of given system of equations
 * @param m left side of system
 * @param v right side of system
 * @param x particular result
 * @param so solution space
 */
template <typename T> void CEquationUtils<T>::findAllSolutions(
    const CMatrix<T> &m, const CVector<T> &v, CVector<T> &x, CMatrix<T> &so) {

  // 1) Finds positions of LD columns in left side matrix and mark items of
  // solution that correspond to these columns (0 - for particular solution
  // x), (0, 1 - for solution space "so", so as contain only LI vectors)
  //-------------------------------------------------------------------------

  // Iterate through columns of left side matrix
  TIndex i = 0, j = 0;
  while (j < m.getNumOfCols()) {

    // If we are behind last row (we moved down one row only if column was
    // LI) or actual item m[i,j] is zero => we are in LD column
    if (i >= m.getNumOfRows() || m.getItemsPtr()[i][j] == 0) {

      // Iterate through vectors of "so", for every vector select item which
      // position corresponds to position of found LD column and set value of
      // this item to 0 or 1 so as "so" will be LI
      for (TIndex k = 0; k < so.getNumOfCols(); k++) {
        so.set(j, k, k == j - i ? 1 : 0);
      }

      // Saves 0 to item of particular solution, which position corresponds
      // to position of found LD column and move to next column
      x.set(j++, 0);
      continue;
    }
    i++;
    j++;
  }

  // 2) Calculates remaining items of particular solution "x" and "so"
  //-------------------------------------------------------------------------

  // Iterate through rows of left side matrix
  for (i = 0; i < m.getNumOfRows(); i++) {

    // Reverse index ri for reverse direction of iteration (from down to up)
    TIndex ri = m.getNumOfRows() - (i + 1);

    // Create temporary variables: "tmpL" for left side of actual equation,
    // "tmpRx" for corresponding item (in same row) of right side vector,
    // "tmpRso" for corresponding items (in same row) of vectors from "so" and
    // set them to zero
    TIndex index(0);
    T tmpL(0), tmpRx(v.getItemsPtr()[ri]), tmpRso[so.getNumOfCols()];
    for (j = 0; j < so.getNumOfCols(); j++) {
      tmpRso[j] = 0;
    }

    // Iterate through columns
    for (j = 0; j < m.getNumOfCols(); j++) {

      // If actual item is zero, move to next item
      if (m.getItemsPtr()[ri][j] == 0) {
        continue;
      }

      // If first nonzero item in actual row has been found, this item is
      // left side of equation, move to next item
      if (!tmpL) {
        tmpL = m.getItemsPtr()[ri][j];
        index = j;
        continue;
      }

      // Other nonzero items are part of the right side ...multiply actual
      // nonzero item by corresponding items from "x" and "so" and subtract
      // results from right sides ("tmpRx", all "tmpRso[k]")
      tmpRx -= m.getItemsPtr()[ri][j] * x.getItemsPtr()[j];
      for (TIndex k = 0; k < so.getNumOfCols(); k++) {
        tmpRso[k] -= m.getItemsPtr()[ri][j] * so.getItemsPtr()[j][k];
      }
    }

    // If left side is zero, move to next item (it is not necessary to test
    // whether right side zero, because option "hasn't solution" was already
    // excluded)
    if (!tmpL) {
      continue;
    }

    // Calculate items of particular result and "so" for actual row
    x.set(index, tmpRx / tmpL);
    for (j = 0; j < so.getNumOfCols(); j++) {
      so.set(index, j, tmpRso[j] / tmpL);
    }
  }
}
//---------------------------------------------------------------------------

/**
 * findSolutionU - solves system of linear equations with square matrix
 * @param m left side of system (square upper triangular matrix)
 * @param v right side of system
 * @param x result
 */
template <typename T> void CEquationUtils<T>::findSolutionU(
    const CMatrix<T> &m, const CVector<T> &v, CVector<T> &x) {

  // Result
  x = CVector<T>(m.getNumOfRows());

  // Iterate through rows of left side matrix
  for (TIndex i = 0; i < m.getNumOfRows(); i++) {

    // Reverse index ri for reverse direction of iteration (from down to up)
    TIndex ri(m.getNumOfRows() - (i + 1));

    // Substitution for actual row
    T sum(0);
    for (TIndex j = ri + 1; j < m.getNumOfCols(); j++) {
      sum += m.get(ri, j) * x.get(j);
    }

    // Solve equation
    x.set(ri, (v.get(ri) - sum) / m.get(ri, ri));
  }
}
//---------------------------------------------------------------------------

/**
 * findSolutionL - solves system of linear equations with square matrix
 * @param m left side of system (square lower triangular matrix)
 * @param v right side of system
 * @param x result
 */
template <typename T> void CEquationUtils<T>::findSolutionL(
    const CMatrix<T> &m, const CVector<T> &v, CVector<T> &x) {

  // Result
  x = CVector<T>(m.getNumOfRows());

  // Iterate through rows of left side matrix
  for (TIndex i = 0; i < m.getNumOfRows(); i++) {

    // Substitution for actual row
    T sum(0);
    for (TIndex j = 0; j < i; j++) {
      sum += m.get(i, j) * x.get(j);
    }

    // Solve equation
    x.set(i, (v.get(i) - sum) / m.get(i, i));
  }
}
//---------------------------------------------------------------------------

template class CEquationUtils<char>;
template class CEquationUtils<short>;
template class CEquationUtils<int>;
template class CEquationUtils<long>;
template class CEquationUtils<float>;
template class CEquationUtils<double>;
template class CEquationUtils<CBigInt>;
template class CEquationUtils<CFraction>;
